//-------------------------
#include "main.h"
#include <Arduino.h>
#include <WiFi.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
//-------定义电机控制接口
#define AIN1 2
#define AIN2 14
#define BIN1 15
#define BIN2 13



//--------定义CAM接口(cam内部针脚未引出)
#define PWDN_GPIO_NUM     32
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM      0
#define SIOD_GPIO_NUM     26
#define SIOC_GPIO_NUM     27

#define Y9_GPIO_NUM       35
#define Y8_GPIO_NUM       34
#define Y7_GPIO_NUM       39
#define Y6_GPIO_NUM       36
#define Y5_GPIO_NUM       21
#define Y4_GPIO_NUM       19
#define Y3_GPIO_NUM       18
#define Y2_GPIO_NUM        5
#define VSYNC_GPIO_NUM    25
#define HREF_GPIO_NUM     23
#define PCLK_GPIO_NUM     22


static camera_config_t camera_config = {
  .pin_pwdn = PWDN_GPIO_NUM,
  .pin_reset = RESET_GPIO_NUM,
  .pin_xclk = XCLK_GPIO_NUM, 
  .pin_sscb_sda = SIOD_GPIO_NUM,  
  .pin_sscb_scl = SIOC_GPIO_NUM,
     
  .pin_d7 = Y9_GPIO_NUM,
  .pin_d6 = Y8_GPIO_NUM,
  .pin_d5 = Y7_GPIO_NUM, 
  .pin_d4 = Y6_GPIO_NUM,
  .pin_d3 = Y5_GPIO_NUM,
  .pin_d2 = Y4_GPIO_NUM,  
  .pin_d1 = Y3_GPIO_NUM,
  .pin_d0 = Y2_GPIO_NUM,
  .pin_vsync = VSYNC_GPIO_NUM,
  .pin_href = HREF_GPIO_NUM,
  .pin_pclk = PCLK_GPIO_NUM,

  .xclk_freq_hz = 20000000,
  .ledc_timer = LEDC_TIMER_0,
  .ledc_channel = LEDC_CHANNEL_0,
  
  .pixel_format = PIXFORMAT_JPEG,
  .frame_size = FRAMESIZE_SVGA,
  .jpeg_quality = 20,
  .fb_count = 1,
};


//void setup() {
  //camera_init();
  //Serial.print("sys is running!");
//}
//int buttonState = 0;
//int btnHold = 0;

//---------------wifi ap模式
const char *ssid ="wang";
const char *password ="12345678";
WiFiServer server(100); //设置监听端口号

void setup(){
  //-------定义电机控制接口模式
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  camera_init();//初始化摄像头
  Serial.begin(9600);
  /*----------sta模式-------------
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.println("Connecting to WiFi..");
  }
  Serial.println("Connected to the WiFi network");
  Serial.println(WiFi.localIP());
  */

  //-----------ap模式------------
  WiFi.softAP(ssid,password);
  Serial.print("IPaddress:");
  Serial.println(WiFi.softAPIP());
  //-------------------------------
  server.begin(); //启用端口
  Serial.println("server started");
  delay(100);
  //xTaskCreate(kz,"kz",20840,NULL,2,NULL);   //使用时留意栈空间
  //xTaskCreate(cam,"cam",2084,NULL,2,NULL);   //使用时留意栈空间
}

void loop(){/*多线程中无需使用*/
    WiFiClient client = server.available();  //实例客户端对象
    while (client)
    {
        camera_fb_t * fb = esp_camera_fb_get();
        if (!fb) {}
        client.write(fb->buf,fb->len);
        esp_camera_fb_return(fb);
        if (client.available()) {
          char c = client.read();
          switch(c){
            case 'L' :Left();Serial.println("left"); break;
            case 'R' :Right();Serial.println("right"); break;
            case 'W' :ForWard();Serial.println("forward"); break;
            case 'B' :BackWard();Serial.println("backward"); break;
          }
        }else{
          delay(300);
          digitalWrite(AIN1,LOW);
          digitalWrite(AIN2,LOW);
          digitalWrite(BIN1,LOW);
          digitalWrite(BIN2,LOW);
          Serial.println("default");
        }
    }

}

//网络控制函数
void kz(void *pvParameters){
  //使用多线程时留意使用死循环
  for(;;){
    //vTaskDelete(tesk1);  //删除任务，参数为NULL时表示删除当前执行的任务
    }
  }


void cam(void *pvParameters){
}

void Right(){
  digitalWrite(AIN1,LOW);
  digitalWrite(AIN2,HIGH);

}
void Left(){
  digitalWrite(AIN1,HIGH);
  digitalWrite(AIN2,LOW);
}

void ForWard(){
  digitalWrite(BIN1,LOW);
  digitalWrite(BIN2,HIGH);
}
void BackWard(){
  digitalWrite(BIN1,HIGH);
  digitalWrite(BIN2,LOW);
}

//------------------摄像头初始化
esp_err_t camera_init(){
    //initialize the camera
    esp_err_t err = esp_camera_init(&camera_config);
    if (err != ESP_OK) {
        Serial.print("Camera Init Failed");
        return err;
    }
    sensor_t * s = esp_camera_sensor_get();
    //initial sensors are flipped vertically and colors are a bit saturated
    if (s->id.PID == OV2640_PID) {
        s->set_vflip(s, 1);//flip it back
        s->set_brightness(s, 1);//up the blightness just a bit
        s->set_contrast(s, 1);
    }
    Serial.print("Camera Init OK");
    return ESP_OK;
}





/*  接收串口数据
void setlora(String a){
  Serial.print(a);
  String d;
  char c;
  // put your main code here, to run repeatedly:
   while (Serial.available() > 0)  
    {
      c = Serial.read();
      d += c; 
    }
}

*/